
#ifndef ROBOT_TRAJECTORY
#define ROBOT_TRAJECTORY

/** Trajectory
 * A joint trajectory is a time series of joint set points which linearly interpolates
 * from one set of desired joint positions to the next based on the joint which must 
 * move the furthest
 * such that they all reach their target at the same time.
 * It can provide the setpoint input for the PID controller
 */

#define NUM_JOINTS 4

#include <openrave-core.h>
#include <vector>
#include <iostream>
#include "robotStruct.hpp"

class Trajectory {
 public:
  Trajectory();
  ~Trajectory();
  
  void addSetpoint(Setpoint* sp);
  uint32_t getNumSetpoints() { return series_->size(); }
  
  /** provide next set point std::vector based on time since init
   *  interpolating against the slowest joint
   */
  void getSetpoint(uint64_t time, Setpoint* sp);
  
 private:
  std::vector<Setpoint*>* series_;
  double last_time_;
};

inline Trajectory::Trajectory() {
  series_ = new std::vector<Setpoint*>();
  last_time_ = -1.0;
}

inline Trajectory::~Trajectory() {
  for(uint32_t i = 0; i < series_->size(); i ++) {
    delete series_->at(i);
  }
  delete series_;
}

inline void Trajectory::addSetpoint(Setpoint* sp) {
  // Copy a new setpoint from given:
  Setpoint* setpoint = new Setpoint(sp);
  series_->push_back(setpoint);
  
  setpoint->cpos->println();
  //printf("\n");
}

inline void Trajectory::getSetpoint(uint64_t time, Setpoint* sp) {
  
  assert(sp != NULL);
  
  sp->jpos->clear();    // rad
  sp->jvel->clear();    // rad/s
  sp->jacc->clear();    // rad/s2
  sp->cpos->clear();    // m
  sp->cvel->clear();    // m/s
  sp->cacc->clear();    // m/s2
  
  // find entry in table which has a later time than requested:
  uint32_t index = 0;
  bool done = false;
  while(index < series_->size() && !done) {
    if(series_->at(index)->time >= time) {
      done = true;
    } else {
      index ++;
    }
  }
  
  // Lower bound:
  if(index <= 0) {
    sp->copy(series_->at(0));
    return;
  }
  
  // Upper bound:
  if(index >= series_->size()) {
    sp->copy(series_->at(series_->size()-1));
    return;
  }
  
  // For all except for jpos dont interpolate for now!
  sp->copy(series_->at(index-1));

  // TODO a handy interpolation function...
  
  // Interpolate angle between angle0 and angle1 for time between t0 and t1:
  uint32_t t0 = series_->at(index-1)->time;
  uint32_t t1 = series_->at(index)->time;
  
  for(uint32_t j = 0; j < NUM_JOINTS; j ++) {
    double angle0 = series_->at(index-1)->jpos->get(j);
    double angle1 = series_->at(index)->jpos->get(j);
    double angle = (angle1-angle0)*(time-t0)/(t1-t0)+angle0;
    sp->jpos->set(j, angle);
  }
}

#endif
